#!/usr/bin/env python

import roslib; roslib.load_manifest('framer')
import rospy
import smach
import image_geometry
import message_filters
import actionlib
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import PointStamped, Point
from actionlib_msgs.msg import GoalStatus
import tf
import cv
import math
import copy
from object_manipulator.convert_functions import *
from backgroundProcessor import *

class Interface(smach.State):
   def __init__(self):
      smach.State.__init__(self, outcomes=['success', 'fail', 'aborted'], output_keys=['point'])
      # Flags and global vars
      self.done = False
      self.cv_image = None
      self.process_image = None
      self.active = False
      self.point = PointStamped()
      self.point.header = None
      self.tf = tf.TransformListener()
      self.b = BackgroundProcessor()

      # OpenCv setup
      self.window_name = "Image Window"
      cv.NamedWindow(self.window_name, 1)
      self.bridge = CvBridge()

      
      # Publishers and Subscribers
      self.image_sub = rospy.Subscriber("/wide_stereo/right/image_rect_color", Image, self.callback)

   def execute(self, userdata):
      userdata.point = None
      self.lookat = None
      self.done = False
      self.active = True
      while(not self.done and not rospy.is_shutdown()):
         rospy.sleep(0.01)
      if rospy.is_shutdown():
         self.done = True
         self.active = False
         rospy.loginfo("Keyboard Interrupt received. Aborting")
         cv.DestroyAllWindows()
         return 'aborted'

      self.active = False
      userdata.point = copy.deepcopy(self.point)
      return 'success' 

   def faces(self, face):
      rospy.loginfo("Found face")
      self.face = face.pos

   def showImage(self):
      if self.cv_image == None:
         rospy.logerror("Trying to display an image before it exists! Skipping.")
         return
      rospy.loginfo("background is {0}".format(self.b.process(self.process_image)))
      cv.ShowImage(self.window_name, self.cv_image)
      cv.WaitKey(2) #Any less than 2 and it freezes when clicked... :(
      
   def callback(self, data):
      if rospy.is_shutdown():
         return
      self.point.header = copy.deepcopy(data.header)
      try:
         self.cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
         self.process_image = self.bridge.imgmsg_to_cv(data, "mono8")
      except CvBridgeError, e:
         print e
         return
      self.showImage()

if __name__== "__main__":
   rospy.init_node('debug_interface', log_level=rospy.DEBUG)

   machine = smach.StateMachine(outcomes=['success', 'fail', 'aborted'])
   with machine:
      smach.StateMachine.add('Display Image', Interface(), transitions={'success':'Display Image', 'fail':'fail', 'aborted':'aborted'})
   try:
      rospy.loginfo("Starting state machine execution")
      machine.execute()
      rospy.spin()
   except KeyboardInterrupt:
      rospy.loginfo("Shutting Down")
   cv.DestroyAllWindows()


